#pragma once
#include "rclcpp/rclcpp.hpp"
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
// 2023.12.18 modify by haibo
#include <agv_msgs/msg/agv_mode.hpp>
#include <std_msgs/msg/bool.hpp>
#include "world.h"
#include <tf/tf.h>
#include <thread>
#include <mutex>
#include <dynamic_reconfigure/server.h>
#include <agv_navigate/paramsConfig.h>

using namespace std;

enum class CtrlMode {
    NoneMode = 0,
    ManualMode = 1,
    HalfAutoMode = 2,
    AutoMode = 3
};

enum class RunMode {
    CloseRun = 0,
    SafeRun = 1,
    AutoRun = 2
};

enum class RunState {
    NoneState = 0,
    StartAngleAdjust = 1,
    MoveToTarget = 2,
    FinalAngleAdjust = 3
};

struct TargetPos {
  double dis;
  double x;
  double y;
  double theta; // angle value in degree

  TargetPos() : dis(0), x(0), y(0), theta(0) {}
};

class DriveToPath
{
 public:
    DriveToPath(rclcpp::Node &nh);
    ~DriveToPath();
    bool Init();
 private:
    // 2023.12.18 modify by haibo
    void AgvModeCallback(const agv_msgs::msg::AGVMode::ConstSharedPtr& msg);
    void AllowMoveCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg);
    bool GetBit(int value, int index);
    static void GetCurPosThreadFun(DriveToPath* p);
    static void AgvDrivingThreadFun(DriveToPath* p);

    // void initTarget();
    void UpdateTarget(TargetPos pos);
    TargetPos GetFinallyTargetPoint();
    //计算点到路径的最近距离与最近位置
    TargetPos PointToLine(double x, double y, Path* path);
    TargetPos PointToBezier(double x, double y, Path* path);
    TargetPos PointToSide(double x, double y, Path* path);
    //根据相对位姿信息，实时计算速度值并发布
    void DoDriving();
    //外部设置参数初始化
    void ExternalParamsInit();
    //变量初始化
    void VariableInit();

    bool drive_to_path_init_ok_ = false;
    rclcpp::Node nh_;
    // force control agv to stop topic (to twist_mux)
    ros::Publisher vel_pub_;
    // 2023.12.18 modify by haibo
    ros::Subscriber agvmode_sub_; 
    ros::Subscriber odom_sub_;
    ros::Publisher allow_move_pub_;
    ros::Subscriber allow_move_sub_;
    int cur_ctrl_mode_;
    int last_ctrl_mode_;
    // bool m_isHalfAuto; //半自动状态中
    bool is_on_path_;  //是否已经行驶到路径上了
    bool is_allow_move_;  //是否允许移动
    int cur_state_;  // 0-角度√距离√；1-角度×距离×；2-角度√距离×；3-角度×距离√；
    double cur_pos_x_;  //当前位置x
    double cur_pos_y_;  //当前位置y
    double cur_pos_t_;  //当前位置theta
    //用于阻塞流程，确保顺序：位置更新---目标---差值状态
    bool cur_pos_is_update_;  // 当前位姿已经刷新
    bool cur_target_is_update_;  // 当前目标已经刷新
    bool cur_state_is_update_;  // 当前状态已经刷新
    //实时目标值与相对位置
    double cur_target_x_;  //目标点x
    double cur_target_y_;  //目标点y
    double cur_target_t_;  //当前目标角度
    double end_target_t_;  //最终目标角度
    double cur_dis_;  //距离差
    double cur_ang_;  //角度差
    double end_ang_;  //最终角度差
    int sign_;  //正负系数（用于调整旋转方向）
    double stop_ang_out_; //角度偏差值大阈值
    //外部设置参数
    int mode_;  //0关闭，1安全运行，2自动运行
    double dis_max_;
    double high_speed_;
    double low_speed_;

    std::mutex mutex_;
    std::thread get_cur_pos_thread_; //线程：用于获取当前位姿，并计算与目标点距离角度，更新状态
    std::thread agv_driving_thread_; //线程：用于计算速度
    /*********************
    ** Dynamic Reconfigure
    **********************/
    dynamic_reconfigure::Server<agv_navigate::paramsConfig>* dynamic_reconfigure_server_;
    dynamic_reconfigure::Server<agv_navigate::paramsConfig>::CallbackType dynamic_reconfigure_callback_;
    void ReconfigCB(agv_navigate::paramsConfig &config, uint32_t unused_level);
};

